#ifndef __ROBOT_H__
#define __ROBOT_H__

#include "matrixtransform.h"

class robot{
 public:
 robot(Matrix4 translation) :	lower_left_arm_transform(0), lower_right_arm_transform(0), upper_left_arm_transform(0), upper_left_leg_transform(0), lower_left_leg_transform(0), lower_right_leg_transform(0), upper_right_arm_transform(0), upper_right_leg_transform(0), angle(0.0),	world(0)
		{
			world = new matrixtransform();
		world->matrix = translation;

		matrixtransform *bellytransform = new matrixtransform();
		bellytransform->matrix = Matrix4::createScaleMatrix(1,3,1);
		cube *belly = new cube(5, Vector3(1,0,0));
		bellytransform->add(belly);
		world->add(bellytransform);

		matrixtransform *headtransform = new matrixtransform();
		headtransform->matrix = Matrix4::createTranslationMatrix(0,5,0);
		headtransform->add(new sphere(5, Vector3(0,0.5,1)));
		world->add(headtransform);

		upper_left_arm_transform = new matrixtransform();
		upper_left_arm_transform->matrix = Matrix4::createTranslationMatrix(5,-4,0)*Matrix4::createScaleMatrix(0.5,1,0.5);
		upper_left_arm_transform->add(new cube(5, Vector3(1,0,0)));
		world->add(upper_left_arm_transform);

		lower_left_arm_transform = new matrixtransform();
		lower_left_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-4,0);
		lower_left_arm_transform->add(new cube(5, Vector3(1,1,0)));
		upper_left_arm_transform->add(lower_left_arm_transform);


		upper_right_arm_transform = new matrixtransform();
		upper_right_arm_transform->matrix = Matrix4::createTranslationMatrix(-5,-4,0)*Matrix4::createScaleMatrix(0.5,1,0.5);
		upper_right_arm_transform->add(new cube(5, Vector3(0,1,1)));
		world->add(upper_right_arm_transform);

		lower_right_arm_transform = new matrixtransform();
		lower_right_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-4,0);
		lower_right_arm_transform->add(new cube(5, Vector3(1,1,1)));
		upper_right_arm_transform->add(lower_right_arm_transform);

		double upper_leg_y=-10.0;
		double lower_leg_y=-5.0;

		upper_right_leg_transform = new matrixtransform();
		upper_right_leg_transform->matrix = Matrix4::createTranslationMatrix(-2,upper_leg_y,0)*Matrix4::createScaleMatrix(0.5, 1, 0.5);
		upper_right_leg_transform->add(new cube(5, Vector3(0,1,1)));
		world->add(upper_right_leg_transform);

		lower_right_leg_transform = new matrixtransform();
		lower_right_leg_transform->matrix = Matrix4::createTranslationMatrix(0,lower_leg_y,0);
		lower_right_leg_transform->add(new cube(5, Vector3(0,0.5,1)));
		upper_right_leg_transform->add(lower_right_leg_transform);

		upper_left_leg_transform = new matrixtransform();
		upper_left_leg_transform->matrix = Matrix4::createTranslationMatrix(2,upper_leg_y,0)*Matrix4::createScaleMatrix(0.5,1,0.5);
		upper_left_leg_transform->add(new cube(5, Vector3(0,0,1)));
		world->add(upper_left_leg_transform);

		lower_left_leg_transform = new matrixtransform();
		lower_left_leg_transform->matrix = Matrix4::createTranslationMatrix(0,lower_leg_y,0);
		lower_left_leg_transform->add(new cube(5, Vector3(0,1,0)));
		upper_left_leg_transform->add(lower_left_leg_transform);
	}
	
	void tick(double dt) {
		static double upper_left_leg_angle=0.0;
		static double left_direction=1;
		upper_left_leg_angle+=0.001*left_direction;
		
		if (upper_left_leg_angle >= acos(-1)/8.0 || upper_left_leg_angle <= -acos(-1)/8.0)
			left_direction = -left_direction;
		
		if (left_direction < 0) {
			upper_left_leg_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * upper_left_leg_transform->matrix;
			lower_left_leg_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_left_leg_transform->matrix;
		}
		else {
			upper_left_leg_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * upper_left_leg_transform->matrix;
			lower_left_leg_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_left_leg_transform->matrix;
		}
		
		
		if (left_direction > 0) {
			upper_right_leg_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * upper_right_leg_transform->matrix;
			lower_right_leg_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_right_leg_transform->matrix;
		}
		else {
			upper_right_leg_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * upper_right_leg_transform->matrix;
			lower_right_leg_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_right_leg_transform->matrix;
		}
		
		
		
		if (left_direction < 0) {
			upper_left_arm_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * upper_left_arm_transform->matrix;
			lower_left_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_left_arm_transform->matrix;
		}
		else {
			upper_left_arm_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * upper_left_arm_transform->matrix;
			lower_left_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_left_arm_transform->matrix;
		}
		
		
		if (left_direction > 0) {
			upper_right_arm_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * upper_right_arm_transform->matrix;
			lower_right_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_right_arm_transform->matrix;
		}
		else {
			upper_right_arm_transform->matrix =  Matrix4::createRotationMatrix(Vector3(1,0,0), -dt) * upper_right_arm_transform->matrix;
			lower_right_arm_transform->matrix = Matrix4::createTranslationMatrix(0,-15,0)*Matrix4::createRotationMatrix(Vector3(1,0,0), dt) * Matrix4::createTranslationMatrix(0,15,0)*lower_right_arm_transform->matrix;
		}
		
		
	}
	
	void draw(FrustumMath fmath) {
		world->draw(&Matrix4::identityMatrix, fmath);
	}
 private:
	matrixtransform *lower_left_arm_transform;
	matrixtransform *lower_right_arm_transform;
	matrixtransform *upper_left_arm_transform;
	matrixtransform *upper_left_leg_transform;
	matrixtransform *lower_left_leg_transform;
	matrixtransform *lower_right_leg_transform;
	matrixtransform *upper_right_arm_transform;
	matrixtransform *upper_right_leg_transform;
	
	double angle;

	matrixtransform *world;
};



#endif
